#include "head_task.h"
#include "MSE_PID.h"
#include "MY_define.h"
#include "MSE_CAN.h"
#include "can.h"
#include "MSE_DBUS.h"
#include "FFC.h"
#include "MSE_ROOT_INIT.h"
#include "main.h"

uint8_t MSE_F_HEAD_AIM_INIT(void)
{
    if (

                MSE_ROOT.STATUS.MOTOR_HEAD_Yaw  == MSE_DF_OFFLINE||
                MSE_ROOT.STATUS.MOTOR_HEAD_Left == MSE_DF_OFFLINE||
                MSE_ROOT.STATUS.MOTOR_HEAD_Right== MSE_DF_OFFLINE||
                MSE_ROOT.STATUS.MOTOR_HEAD_SWAY == MSE_DF_OFFLINE||
//	            MSE_ROOT.STATUS.MOTOR_HEAD_LIFT_L  == MSE_DF_OFFLINE||
//	            MSE_ROOT.STATUS.MOTOR_HEAD_LIFT_R  == MSE_DF_OFFLINE
                MSE_ROOT.STATUS.MOTOR_HEAD_HORIZONTAL_L        == MSE_DF_OFFLINE||
                MSE_ROOT.STATUS.MOTOR_HEAD_HORIZONTAL_R        == MSE_DF_OFFLINE
//        	       

	    )

        return MSE_DF_ERROR;

    if (HAL_GetTick() < 5000)
    {
        return MSE_DF_ERROR;
    }

    MSE_F_HEAD_MOTOR_CLEAR(&MSE_V_MOTOR_Yaw);
    MSE_F_HEAD_MOTOR_CLEAR(&MSE_V_MOTOR_L_CUP_PUMP);
    MSE_F_HEAD_MOTOR_CLEAR(&MSE_V_MOTOR_R_CUP_PUMP);
    MSE_F_HEAD_MOTOR_CLEAR(&MSE_V_MOTOR_Pitch);
    MSE_F_HEAD_MOTOR_CLEAR(&MSE_V_MOTOR_SWAY);
	MSE_F_HEAD_MOTOR_CLEAR(&MSE_V_MOTOR_LIFT_L);
	MSE_F_HEAD_MOTOR_CLEAR(&MSE_V_MOTOR_LIFT_R);
	MSE_F_HEAD_MOTOR_CLEAR(&MSE_V_MOTOR_HORIZONTAL_L);
	MSE_F_HEAD_MOTOR_CLEAR(&MSE_V_MOTOR_HORIZONTAL_R);


    MSE_V_MOTOR_BOTTOM_3508_1.DATA.Aim = 0;
    MSE_V_MOTOR_BOTTOM_3508_2.DATA.Aim = 0;
    MSE_V_MOTOR_BOTTOM_3508_3.DATA.Aim = 0;
    MSE_V_MOTOR_BOTTOM_3508_4.DATA.Aim = 0;

    MSE_V_MOTOR_BOTTOM_3508_1.PID_S.I_Lit = 0;
    MSE_V_MOTOR_BOTTOM_3508_1.PID_C.I_Lit = 0;

    MSE_V_MOTOR_BOTTOM_3508_2.PID_S.I_Lit = 0;
    MSE_V_MOTOR_BOTTOM_3508_2.PID_C.I_Lit = 0;

    MSE_V_MOTOR_BOTTOM_3508_3.PID_S.I_Lit = 0;
    MSE_V_MOTOR_BOTTOM_3508_3.PID_C.I_Lit = 0;

    MSE_V_MOTOR_BOTTOM_3508_4.PID_S.I_Lit = 0;
    MSE_V_MOTOR_BOTTOM_3508_4.PID_C.I_Lit = 0;
    return MSE_DF_READY;
}

uint8_t heading(uint8_t DBUS_STATUS)
{

    if (PID_INIT != MSE_DF_READY)
    {
        PID_INIT = MSE_F_MOTOR_PID_HEAD_INIT();
        return MSE_DF_ERROR;
    }
    if (AIM_INIT != MSE_DF_READY)
    {
        AIM_INIT = MSE_F_HEAD_AIM_INIT();
        return MSE_DF_ERROR;
    }
  	   

      
       

    if (MSE_V_CONTAL.MOD == 0) // S1  1		S2 3
    {

        MSE_V_MOTOR_Yaw.DATA.Aim = MSE_F_MATH_Limit_float(first_power + 4090, first_power, MSE_V_CONTAL.HEAD.Yaw);
        if (MSE_V_MOTOR_Yaw.DATA.Aim > first_power + 550 && MSE_V_MOTOR_Yaw.DATA.Aim < 3500 + first_power)
        {
            MSE_V_MOTOR_Pitch.DATA.Aim = MSE_F_MATH_Limit_float(1.6, -1.6, MSE_V_CONTAL.HEAD.Pitch);
        }
        else
        {
            MSE_V_MOTOR_Pitch.DATA.Aim = MSE_F_MATH_Limit_float(1.6, 0.2, MSE_V_CONTAL.HEAD.Pitch);
        }
        MSE_V_MOTOR_L_CUP_PUMP.DATA.Aim     = +MSE_V_CONTAL.HEAD.A_CUP_PUMP+MSE_V_CONTAL.HEAD.R_CUP_PUMP;
	    MSE_V_MOTOR_R_CUP_PUMP.DATA.Aim     = -MSE_V_CONTAL.HEAD.A_CUP_PUMP+MSE_V_CONTAL.HEAD.R_CUP_PUMP;
		MSE_V_MOTOR_HORIZONTAL_L.DATA.Aim= MSE_V_CONTAL.HEAD.HORIZONTAL;
		MSE_V_MOTOR_HORIZONTAL_R.DATA.Aim=-MSE_V_CONTAL.HEAD.HORIZONTAL;
		MSE_V_MOTOR_SWAY.DATA.Aim = MSE_V_CONTAL.HEAD.SWAY;
        YAW_MOTOR_HOME();
    }

    else if (MSE_V_CONTAL.MOD == 1) //  S1  1	S2 2
    {

        MSE_V_MOTOR_Yaw.DATA.Aim = MSE_F_MATH_Limit_float(first_power + 4090, first_power, MSE_V_CONTAL.HEAD.Yaw);
        if (MSE_V_MOTOR_Yaw.DATA.Aim > 650 && MSE_V_MOTOR_Yaw.DATA.Aim < 3500)
        {
            MSE_V_MOTOR_Pitch.DATA.Aim = MSE_F_MATH_Limit_float(1.4, -1.8, MSE_V_CONTAL.HEAD.Pitch)+0.2;
        }
        else
        {
            MSE_V_MOTOR_Pitch.DATA.Aim = MSE_F_MATH_Limit_float(1.4, 0, MSE_V_CONTAL.HEAD.Pitch)+0.2;
        }
        MSE_V_MOTOR_L_CUP_PUMP.DATA.Aim = +MSE_V_CONTAL.HEAD.A_CUP_PUMP + MSE_V_CONTAL.HEAD.R_CUP_PUMP;
        MSE_V_MOTOR_R_CUP_PUMP.DATA.Aim = -MSE_V_CONTAL.HEAD.A_CUP_PUMP + MSE_V_CONTAL.HEAD.R_CUP_PUMP;
		MSE_V_MOTOR_HORIZONTAL_L.DATA.Aim= MSE_V_CONTAL.HEAD.HORIZONTAL;
		MSE_V_MOTOR_HORIZONTAL_R.DATA.Aim=-MSE_V_CONTAL.HEAD.HORIZONTAL;
		MSE_V_MOTOR_SWAY.DATA.Aim = MSE_V_CONTAL.HEAD.SWAY;
        YAW_MOTOR_HOME();
    }

    //
    if (!DBUS_STATUS)
    {
        MSE_V_MOTOR_HORIZONTAL_L.PID_P.I_Lit = 0;
        MSE_V_MOTOR_HORIZONTAL_L.PID_S.I_Lit = 0;

        MSE_V_MOTOR_HORIZONTAL_R.PID_P.I_Lit = 0;
        MSE_V_MOTOR_HORIZONTAL_R.PID_S.I_Lit = 0;

        MSE_V_MOTOR_LIFT_L.PID_P.I_Lit = 0;
        MSE_V_MOTOR_LIFT_L.PID_S.I_Lit = 0;

        MSE_V_MOTOR_LIFT_R.PID_P.I_Lit = 0;
        MSE_V_MOTOR_LIFT_R.PID_S.I_Lit = 0;

        MSE_V_MOTOR_SWAY.PID_P.I_Lit = 0;
        MSE_V_MOTOR_SWAY.PID_S.I_Lit = 0;

        MSE_V_MOTOR_L_CUP_PUMP.PID_P.I_Lit = 0;
        MSE_V_MOTOR_L_CUP_PUMP.PID_S.I_Lit = 0;

        MSE_V_MOTOR_R_CUP_PUMP.PID_P.I_Lit = 0;
        MSE_V_MOTOR_R_CUP_PUMP.PID_S.I_Lit = 0;

        MSE_V_MOTOR_Yaw.PID_P.I_Lit = 0;
        MSE_V_MOTOR_Yaw.PID_S.I_Lit = 0;

        MSE_V_MOTOR_Camera.PID_P.I_Lit = 0;
        MSE_V_MOTOR_Camera.PID_S.I_Lit = 0;

        MSE_V_MOTOR_BOTTOM_3508_1.PID_S.I_Lit = 0;
        MSE_V_MOTOR_BOTTOM_3508_1.PID_C.I_Lit = 0;

        MSE_V_MOTOR_BOTTOM_3508_2.PID_S.I_Lit = 0;
        MSE_V_MOTOR_BOTTOM_3508_2.PID_C.I_Lit = 0;

        MSE_V_MOTOR_BOTTOM_3508_3.PID_S.I_Lit = 0;
        MSE_V_MOTOR_BOTTOM_3508_3.PID_C.I_Lit = 0;

        MSE_V_MOTOR_BOTTOM_3508_4.PID_S.I_Lit = 0;
        MSE_V_MOTOR_BOTTOM_3508_4.PID_C.I_Lit = 0;
        /***********************************************************************************************************/
        MSE_V_MOTOR_HORIZONTAL_L.DATA.Aim = MSE_V_MOTOR_HORIZONTAL_L.DATA.Angle_Infinite;
        MSE_V_MOTOR_HORIZONTAL_R.DATA.Aim = MSE_V_MOTOR_HORIZONTAL_R.DATA.Angle_Infinite;

        MSE_V_MOTOR_LIFT_L.DATA.Aim = MSE_V_MOTOR_LIFT_L.DATA.Angle_Infinite;
        MSE_V_MOTOR_LIFT_R.DATA.Aim = MSE_V_MOTOR_LIFT_R.DATA.Angle_Infinite;

        MSE_V_MOTOR_SWAY.DATA.Aim = MSE_V_MOTOR_SWAY.DATA.Angle_Infinite;

        MSE_V_MOTOR_L_CUP_PUMP.DATA.Aim = MSE_V_MOTOR_L_CUP_PUMP.DATA.Angle_Infinite;
        MSE_V_MOTOR_R_CUP_PUMP.DATA.Aim = MSE_V_MOTOR_R_CUP_PUMP.DATA.Angle_Infinite;

        MSE_V_MOTOR_Yaw.DATA.Aim = MSE_V_MOTOR_Yaw.DATA.Angle_Infinite;

        MSE_V_MOTOR_Camera.DATA.Aim = MSE_V_MOTOR_Camera.DATA.Angle_Infinite;

        CHASSIS_VX = 0;
        CHASSIS_VY = 0;
        CHASSIS_VW = 0;
    }

    MSE_F_HEAD_MOTOR3508_STUCK(&MSE_V_MOTOR_HORIZONTAL_L,2500,10);
	MSE_F_HEAD_MOTOR3508_STUCK(&MSE_V_MOTOR_HORIZONTAL_R,2500,10);
	MSE_F_HEAD_MOTOR3508_STUCK(&MSE_V_MOTOR_L_CUP_PUMP,25000,10);
	MSE_F_HEAD_MOTOR3508_STUCK(&MSE_V_MOTOR_R_CUP_PUMP,25000,10);
	MSE_F_HEAD_MOTOR3508_STUCK(&MSE_V_MOTOR_SWAY,4000,10);
    MSE_F_HEAD_MOTOR3508_STUCK(&MSE_V_MOTOR_Yaw,25000,10);

	Feedforward_Calculate(&FFC_ALL.LIFT_L , MSE_V_MOTOR_LIFT_L.DATA.Aim);
	Feedforward_Calculate(&FFC_ALL.LIFT_R , MSE_V_MOTOR_LIFT_R.DATA.Aim);
    Feedforward_Calculate(&FFC_ALL.Yaw, MSE_V_MOTOR_Yaw.DATA.Aim);
    Feedforward_Calculate(&FFC_ALL.CUP_PUMP_L , MSE_V_MOTOR_L_CUP_PUMP.DATA.Aim);
	Feedforward_Calculate(&FFC_ALL.CUP_PUMP_R , MSE_V_MOTOR_R_CUP_PUMP.DATA.Aim);
    Feedforward_Calculate(&FFC_ALL.SWAY, MSE_V_MOTOR_SWAY.DATA.Aim);
    Feedforward_Calculate(&FFC_ALL.HORIZONTAL_L , MSE_V_MOTOR_HORIZONTAL_L.DATA.Aim);
	Feedforward_Calculate(&FFC_ALL.HORIZONTAL_R , MSE_V_MOTOR_HORIZONTAL_R.DATA.Aim);
	
	MSE_F_MOTOR_PID_TWO_3508RM(&MSE_V_MOTOR_HORIZONTAL_L);
	MSE_F_MOTOR_PID_TWO_3508RM(&MSE_V_MOTOR_HORIZONTAL_R);
    MSE_F_MOTOR_PID_TWO_3508RM(&MSE_V_MOTOR_Yaw);
    MSE_F_MOTOR_PID_TWO_3508RM(&MSE_V_MOTOR_Pitch);
    MSE_F_MOTOR_PID_TWO_3508RM(&MSE_V_MOTOR_L_CUP_PUMP);
    MSE_F_MOTOR_PID_TWO_3508RM(&MSE_V_MOTOR_R_CUP_PUMP);
    MSE_F_MOTOR_PID_TWO_3508RM(&MSE_V_MOTOR_SWAY);

    MSE_F_MOTOR_CHSISS_3508RM(&MSE_V_MOTOR_BOTTOM_3508_1);
    MSE_F_MOTOR_CHSISS_3508RM(&MSE_V_MOTOR_BOTTOM_3508_2);
    MSE_F_MOTOR_CHSISS_3508RM(&MSE_V_MOTOR_BOTTOM_3508_3);
    MSE_F_MOTOR_CHSISS_3508RM(&MSE_V_MOTOR_BOTTOM_3508_4);

    return MSE_DF_READY;
}



uint8_t MSE_F_TOTAL_CONTRAL(void)
{
    MSE_V_DBUS_STATUS = MSE_ROOT.STATUS.RM_DBUS;

    switch (MSE_ROOT.MASTER_LOCATION)
    {
    case MSE_DF_MASTER_LOCATION_HEAD:
    {
        heading(MSE_V_DBUS_STATUS);
        
        return MSE_DF_READY;
    };
    }
    m++;
    return MSE_DF_ERROR;
}